#include "ipm.h"
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc/types_c.h>
#include <base/geometry.h>

#ifndef EPS
#define EPS 1e-12
#endif

using namespace LFM;

void IPM::CalMatrix(const Camera &camera)
{
    Eigen::Matrix3d rcw = camera.mount.rotation.toRotationMatrix().transpose();
    Eigen::Vector3d tcw = -rcw * camera.mount.translation;
    Eigen::Vector4d ground_plane(0.0, 0.0, -1.0, 0.0); 

    Eigen::Vector3d t = camera.K * tcw;
    Eigen::Matrix3d p = camera.K * rcw;

    b_ = Eigen::Vector4d(-t.x(), -t.y(), -t.z(), -ground_plane.w());
    A_ << p(0, 0), p(0, 1), p(0, 2), 0.0,
        p(1, 0), p(1, 1), p(1, 2),  0.0,
        p(2, 0), p(2, 1), p(2, 2), -1.0,
        ground_plane[0], ground_plane[1], ground_plane[2], 0.0;
}

 Eigen::Vector3d IPM::Mapping(const Eigen::Vector2d& uv)
 {
     A_(0, 3) = -uv.x();
     A_(1, 3) = -uv.y();
     Eigen::Vector4d X = A_.inverse() * b_;
     return Eigen::Vector3d(X(0), X(1), X(2));
 }

 /*bool IPM::CalVP(const std::vector<DlLane> &dlLanes, Eigen::Vector2d &vanish_pt)
 {
     std::vector<std::vector<cv::Point2d>> valid_lines2d;
     for (DlLane dlLane : dlLanes)
     {
         std::vector<cv::Point2d> filtered_geometry;
         cv::Rect rect = cv::boundingRect(dlLane.polygon);
         for (std::size_t i = 0; i < dlLane.polygon.size(); i++)
         {
             if (dlLane.polygon[i].y > rect.y + rect.height * 0.5 && dlLane.polygon[i].y < rect.y + rect.height * 0.9)
             {
                 filtered_geometry.emplace_back(dlLane.polygon[i]);
             }
         }

         cv::Rect remian_rect = cv::boundingRect(filtered_geometry);
         if (remian_rect.height > 100)
         {
             valid_lines2d.emplace_back(filtered_geometry);
         }
     }

     if (valid_lines2d.empty())
     {
         return false;
     }

     int n = valid_lines2d.size();
     std::vector<Eigen::Vector3d> line_args(n);
     for (std::size_t i = 0; i < valid_lines2d.size(); i++)
     {
         cv::Vec4f cv_args;
         cv::fitLine(valid_lines2d[i],
                     cv_args,
                     CV_DIST_L2,
                     0.0, 0.01, 0.01);

         double a = cv_args[0], b = -cv_args[1], c = -a * cv_args[2] + b * cv_args[3];
         line_args[i] = Eigen::Vector3d(a, b, c);
     }

     double delta = 0.0;
     Eigen::MatrixXd A = Eigen::MatrixXd::Zero(n, 3);
     Eigen::MatrixXd W = Eigen::MatrixXd::Identity(n, n);
     Eigen::VectorXd X = Eigen::VectorXd::Zero(3);

     auto robustWeightCauchy = [=](double norm_res) -> double {
         return 1.0 / (1.0 + norm_res * norm_res);
     };

     do
     {
         for (int i = 0; i < n; i++)
         {
             A(i, 0) = line_args[i].x();
             A(i, 1) = line_args[i].y();
             A(i, 2) = line_args[i].z();
         }
         Eigen::MatrixXd C = A.transpose() * W * A;
         Eigen::EigenSolver<Eigen::MatrixXd> es(C);
         Eigen::EigenSolver<Eigen::MatrixXd>::EigenvalueType evs = es.eigenvalues();

         int min_coff = 0;
         evs.real().minCoeff(&min_coff);
         Eigen::VectorXd vx = es.eigenvectors().col(min_coff).real();

         Eigen::VectorXd err_X = A * X;
         for (int i = 0; i < n; i++)
         {
             W(i, i) = robustWeightCauchy(err_X[i]);
         }

         delta = (vx - X).norm();
         X = vx;

     } while (delta > EPS);

     if(std::fabs(X[2]) > EPS)
     {
        X /= X[2];
        vanish_pt = Eigen::Vector3d(X[0], X[1]);
        return true;
     }
     return false;
 }*/